﻿using UnityEngine;

public class FlowFieldAgent : MonoBehaviour
{
    [Header("Navigation Settings")]
    public FlowFieldPathfinding flowField;
    public float moveSpeed = 5f;
    public float rotationSpeed = 10f;
    
    [Header("Agent Physics")]
    public float agentRadius = 0.5f;
    public float collisionForce = 10f;
    public float collisionDamping = 0.8f;
    
    [Header("Target Squeezing")]
    public float targetSqueezeRadius = 2f;
    public float squeezeForce = 5f;
    public float squeezeDamping = 0.9f;
    
    [Header("Collision Avoidance")]
    public float collisionCheckDistance = 1f;
    public float collisionAvoidanceForce = 10f;
    public float collisionAvoidanceAngle = 45f;
    public LayerMask collisionLayer;
    
    [Header("Debug Settings")]
    public bool showDebugGizmos = true;
    
    private Rigidbody rb;
    private TargetAreaManager targetManager;
    private Vector3 assignedTarget;
    private bool hasAssignedTarget;
    private Vector3 antiOscillationTarget;
    private Vector3 squeezeVelocity;
    private int stuckFrames = 0;
    private const int maxStuckFrames = 10;

    private void Start()
    {
        rb = GetComponent<Rigidbody>();
        rb.constraints = RigidbodyConstraints.FreezeRotation | RigidbodyConstraints.FreezePositionY;
        
        targetManager = FindObjectOfType<TargetAreaManager>();
        
        if (flowField == null)
        {
            flowField = FindObjectOfType<FlowFieldPathfinding>();
        }
    }
    
    private void Update()
    {
        if (flowField == null || !flowField.IsInitialized) return;
        
        // 获取目标位置（带防振荡）
        Vector3 targetPosition = GetStableTargetPosition();
        
        // 获取流场方向
        Vector2 flowDirection = flowField.GetFlowDirection(transform.position);
        Vector3 desiredDirection = new Vector3(flowDirection.x, 0, flowDirection.y);
        
        // 添加目标挤压
        Vector3 squeezeDirection = CalculateTargetSqueeze(targetPosition);
        
        // 组合方向
        Vector3 finalDirection = desiredDirection.normalized * 0.8f + 
                                squeezeDirection.normalized * 0.2f;
        
        // 添加碰撞避让
        Vector3 avoidanceDirection = CalculateCollisionAvoidance(finalDirection);
        finalDirection += avoidanceDirection;
        
        // 移动代理
        if (finalDirection != Vector3.zero)
        {
            // 应用移动前检查碰撞
            Vector3 moveVector = finalDirection.normalized * moveSpeed * Time.deltaTime;
            Vector3 newPosition = transform.position + moveVector;
            
            if (IsPositionValid(newPosition))
            {
                // 应用移动
                transform.position = newPosition;
                stuckFrames = 0;
                
                // 平滑旋转
                Quaternion targetRotation = Quaternion.LookRotation(finalDirection);
                transform.rotation = Quaternion.Slerp(
                    transform.rotation, 
                    targetRotation, 
                    rotationSpeed * Time.deltaTime
                );
            }
            else
            {
                // 位置无效，尝试备选移动
                TryAlternativeMovement();
                stuckFrames++;
                
                // 如果卡住太久，尝试重置位置
                if (stuckFrames > maxStuckFrames)
                {
                    ResetToSafePosition();
                }
            }
        }
    }
    
    private void FixedUpdate()
    {
        ApplySqueezeDamping();
        
        // 防止卡在障碍物中
        if (IsStuckInObstacle())
        {
            EscapeFromObstacle();
        }
    }
    
    private Vector3 GetStableTargetPosition()
    {
        if (flowField == null) return transform.position;
        
        Vector3 target = flowField.GridToWorldPosition(flowField.Destination);
        
        // 当接近目标时，锁定目标位置防止振荡
        if (Vector3.Distance(transform.position, target) < 3f)
        {
            if (antiOscillationTarget == Vector3.zero)
            {
                antiOscillationTarget = target;
            }
            return antiOscillationTarget;
        }
        else
        {
            antiOscillationTarget = Vector3.zero;
            return target;
        }
    }
    
    private Vector3 CalculateTargetSqueeze(Vector3 targetPosition)
    {
        float distanceToTarget = Vector3.Distance(transform.position, targetPosition);
        
        if (distanceToTarget < targetSqueezeRadius)
        {
            // 请求分配的槽位
            if (!hasAssignedTarget && targetManager != null)
            {
                assignedTarget = targetManager.AssignSlot(this);
                hasAssignedTarget = true;
            }
            
            // 使用分配的槽位
            if (hasAssignedTarget)
            {
                targetPosition = assignedTarget;
            }
            
            // 计算从目标指向代理的方向
            Vector3 fromTarget = (transform.position - targetPosition).normalized;
            
            // 添加随机扰动避免死锁
            Vector3 randomOffset = new Vector3(
                Random.Range(-0.5f, 0.5f),
                0,
                Random.Range(-0.5f, 0.5f)
            );
            
            return (fromTarget + randomOffset).normalized * squeezeForce;
        }
        
        return Vector3.zero;
    }
    
    private Vector3 CalculateCollisionAvoidance(Vector3 direction)
    {
        Vector3 avoidance = Vector3.zero;
        
        // 向前方发射射线检测碰撞
        if (Physics.Raycast(transform.position, direction, out RaycastHit hit, collisionCheckDistance, collisionLayer))
        {
            // 计算避让方向
            Vector3 hitNormal = hit.normal;
            hitNormal.y = 0; // 确保在水平面
            avoidance = hitNormal.normalized * collisionAvoidanceForce;
            
            // 添加随机扰动避免卡住
            Vector3 randomOffset = new Vector3(
                Random.Range(-0.3f, 0.3f),
                0,
                Random.Range(-0.3f, 0.3f)
            );
            avoidance += randomOffset;
        }
        
        // 检查两侧的碰撞
        Vector3 rightDirection = Quaternion.Euler(0, collisionAvoidanceAngle, 0) * direction;
        if (Physics.Raycast(transform.position, rightDirection, collisionCheckDistance, collisionLayer))
        {
            avoidance += -transform.right * collisionAvoidanceForce * 0.5f;
        }
        
        Vector3 leftDirection = Quaternion.Euler(0, -collisionAvoidanceAngle, 0) * direction;
        if (Physics.Raycast(transform.position, leftDirection, collisionCheckDistance, collisionLayer))
        {
            avoidance += transform.right * collisionAvoidanceForce * 0.5f;
        }
        
        return avoidance;
    }
    
    private bool IsPositionValid(Vector3 position)
    {
        // 检查是否在网格边界内
        var gridPos = flowField.WorldToGridPosition(position);
        if (!flowField.IsPositionValid(gridPos)) 
        {
            return false;
        }
        
        // 检查是否在障碍物上
        if (flowField.IsAnyObstacle(gridPos)) 
        {
            return false;
        }
        
        // 物理检查（更精确的碰撞检测）
        if (Physics.CheckSphere(position, agentRadius, collisionLayer))
        {
            return false;
        }
        
        return true;
    }
    
    private void TryAlternativeMovement()
    {
        // 尝试向侧面移动
        Vector3[] alternativeDirections = {
            transform.right,
            -transform.right,
            transform.forward * 0.5f + transform.right * 0.5f,
            transform.forward * 0.5f - transform.right * 0.5f
        };
        
        foreach (Vector3 dir in alternativeDirections)
        {
            Vector3 newPosition = transform.position + dir.normalized * moveSpeed * Time.deltaTime;
            if (IsPositionValid(newPosition))
            {
                transform.position = newPosition;
                stuckFrames = 0;
                return;
            }
        }
        
        // 所有方向都无效，尝试小步移动
        Vector3 smallStep = transform.forward * moveSpeed * Time.deltaTime * 0.1f;
        Vector3 smallStepPosition = transform.position + smallStep;
        if (IsPositionValid(smallStepPosition))
        {
            transform.position = smallStepPosition;
            stuckFrames = 0;
            return;
        }
        
        // 增加卡住帧数
        stuckFrames++;
    }
    
    private void ResetToSafePosition()
    {
        // 找到最近的安全位置
        Vector3 safePosition = FindNearestSafePosition();
        transform.position = safePosition;
        stuckFrames = 0;
    }
    
    private Vector3 FindNearestSafePosition()
    {
        // 检查当前位置是否安全
        if (IsPositionValid(transform.position))
        {
            return transform.position;
        }
        
        // 在周围寻找安全位置
        float searchRadius = agentRadius * 3f;
        int steps = 8;
        float angleStep = 360f / steps;
        
        for (int i = 0; i < steps; i++)
        {
            float angle = i * angleStep * Mathf.Deg2Rad;
            Vector3 dir = new Vector3(Mathf.Cos(angle), 0, Mathf.Sin(angle));
            Vector3 testPosition = transform.position + dir * searchRadius;
            
            if (IsPositionValid(testPosition))
            {
                return testPosition;
            }
        }
        
        // 没有找到安全位置，返回原点
        return Vector3.zero;
    }
    
    private void ApplySqueezeDamping()
    {
        // 应用挤压速度阻尼
        squeezeVelocity *= squeezeDamping;
        rb.velocity = new Vector3(rb.velocity.x * squeezeDamping, rb.velocity.y, rb.velocity.z * squeezeDamping);
    }
    

    
    private Vector3 CalculateEscapeDirection(Collision collision)
    {
        Vector3 escapeDirection = Vector3.zero;
        
        // 计算平均碰撞法线
        foreach (ContactPoint contact in collision.contacts)
        {
            escapeDirection += contact.normal;
        }
        escapeDirection = escapeDirection.normalized;
        
        // 尝试找到安全的逃生方向
        Vector3[] testDirections = {
            escapeDirection,
            Quaternion.Euler(0, 45, 0) * escapeDirection,
            Quaternion.Euler(0, -45, 0) * escapeDirection,
            Quaternion.Euler(0, 90, 0) * escapeDirection,
            Quaternion.Euler(0, -90, 0) * escapeDirection
        };
        
        foreach (Vector3 dir in testDirections)
        {
            Vector3 testPosition = transform.position + dir.normalized * agentRadius * 2f;
            if (IsPositionValid(testPosition))
            {
                return dir;
            }
        }
        
        // 没有找到安全方向，使用随机方向
        return new Vector3(Random.Range(-1f, 1f), 0, Random.Range(-1f, 1f)).normalized;
    }
    
    private bool IsStuckInObstacle()
    {
        // 检查是否在障碍物内
        return Physics.CheckSphere(transform.position, agentRadius * 1.1f, collisionLayer);
    }
    
    private void EscapeFromObstacle()
    {
        // 尝试多个逃脱方向
        Vector3[] escapeDirections = {
            transform.forward,
            -transform.forward,
            transform.right,
            -transform.right,
            transform.forward + transform.right,
            transform.forward - transform.right,
            -transform.forward + transform.right,
            -transform.forward - transform.right
        };
        
        foreach (Vector3 dir in escapeDirections)
        {
            Vector3 testPosition = transform.position + dir.normalized * agentRadius * 2f;
            if (IsPositionValid(testPosition))
            {
                rb.AddForce(dir * collisionForce * 5f, ForceMode.Impulse);
                return;
            }
        }
        
        // 所有方向都无效，使用随机方向
        Vector3 randomDir = new Vector3(Random.Range(-1f, 1f), 0, Random.Range(-1f, 1f)).normalized;
        rb.AddForce(randomDir * collisionForce * 5f, ForceMode.Impulse);
    }
    
    private void OnDestroy()
    {
        if (targetManager != null && hasAssignedTarget)
        {
            targetManager.ReleaseSlot(this);
        }
    }
    
    private void OnDrawGizmosSelected()
    {
        if (!showDebugGizmos) return;
        
        // 绘制代理半径
        Gizmos.color = Color.blue;
        Gizmos.DrawWireSphere(transform.position, agentRadius);
        
        // 绘制目标挤压范围
        if (flowField != null)
        {
            Vector3 targetPos = flowField.GridToWorldPosition(flowField.Destination);
            Gizmos.color = new Color(1, 0.5f, 0, 0.3f);
            Gizmos.DrawWireSphere(targetPos, targetSqueezeRadius);
        }
        
        // 绘制分配的槽位（如果有）
        if (hasAssignedTarget)
        {
            Gizmos.color = Color.magenta;
            Gizmos.DrawSphere(assignedTarget, 0.3f);
            Gizmos.DrawLine(transform.position, assignedTarget);
        }
        
        // 绘制碰撞检测射线
        Gizmos.color = Color.yellow;
        Gizmos.DrawRay(transform.position, transform.forward * collisionCheckDistance);
        
        Vector3 rightDirection = Quaternion.Euler(0, collisionAvoidanceAngle, 0) * transform.forward;
        Gizmos.DrawRay(transform.position, rightDirection * collisionCheckDistance);
        
        Vector3 leftDirection = Quaternion.Euler(0, -collisionAvoidanceAngle, 0) * transform.forward;
        Gizmos.DrawRay(transform.position, leftDirection * collisionCheckDistance);
    }
}